#ifndef PLANNER_MANAGER_HPP
#define PLANNER_MANAGER_HPP

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/String.h>
#include <string>
#include <map>
#include <memory>
#include "auto_navigation/planning/planner_interface.hpp"

namespace auto_navigation {
namespace planning {

/**
 * @brief 规划器管理器类，用于管理和切换不同的规划算法
 */
class PlannerManager {
public:
    /**
     * @brief 构造函数
     */
    PlannerManager();
    
    /**
     * @brief 析构函数
     */
    ~PlannerManager();
    
    /**
     * @brief 初始化规划器管理器
     * @param nh ROS节点句柄
     * @return 是否成功初始化
     */
    bool initialize(ros::NodeHandle& nh);
    
    /**
     * @brief 注册规划器
     * @param planner 规划器指针
     * @return 是否成功注册
     */
    bool registerPlanner(PlannerInterfacePtr planner);
    
    /**
     * @brief 切换当前使用的规划器
     * @param name 规划器名称
     * @return 是否成功切换
     */
    bool switchPlanner(const std::string& name);
    
    /**
     * @brief 设置地图
     * @param map 占用栅格地图
     */
    void setMap(const nav_msgs::OccupancyGrid& map);

    /**
     * @brief 设置点云数据已接收标志
     * @param received 是否已接收点云数据
     */
    void setPointCloudReceived(bool received = true) { 
        pointcloud_received_ = received;
        if (received) {
            map_received_ = true; // 如果点云已接收，也将地图接收标志设为true
        }
    }
    
    /**
     * @brief 规划路径
     * @param start 起始位姿
     * @param goal 目标位姿
     * @param path 输出的路径
     * @return 是否成功规划
     */
    bool plan(const geometry_msgs::PoseStamped& start,
             const geometry_msgs::PoseStamped& goal,
             nav_msgs::Path& path);
    
    /**
     * @brief 获取当前规划器名称
     * @return 当前规划器名称
     */
    std::string getCurrentPlannerName() const;
    
    /**
     * @brief 获取所有可用规划器名称
     * @return 规划器名称列表
     */
    std::vector<std::string> getAvailablePlanners() const;
    
    /**
     * @brief 根据名称获取规划器
     * @param name 规划器名称
     * @return 规划器指针，如果未找到则返回nullptr
     */
    PlannerInterfacePtr getPlannerByName(const std::string& name) const;
    
private:
    /**
     * @brief 规划器切换回调函数
     * @param msg 包含规划器名称的消息
     */
    void plannerSwitchCallback(const std_msgs::String::ConstPtr& msg);
    
    ros::NodeHandle* nh_;                              ///< ROS节点句柄
    ros::Subscriber planner_switch_sub_;               ///< 规划器切换订阅器
    std::map<std::string, PlannerInterfacePtr> planners_;  ///< 规划器映射表
    PlannerInterfacePtr current_planner_;              ///< 当前使用的规划器
    nav_msgs::OccupancyGrid current_map_;              ///< 当前地图
    bool map_received_;                                ///< 是否接收到地图
    bool pointcloud_received_ = false;                 ///< 是否接收到点云数据
};

} // namespace planning
} // namespace auto_navigation

#endif // PLANNER_MANAGER_HPP 